#include "STECCalculator.h"
#include <cmath>
#include <algorithm>
#include <iostream>

// 常量定义
constexpr double TECU_FACTOR = 40.3e16;  // 1 TECU = 10^16 electrons/m^2
constexpr double LIGHT_SPEED = 299792458.0; // 光速(m/s)
constexpr double PI = 3.14159265358979323846;
constexpr double DEG2RAD = PI / 180.0;
constexpr double RAD2DEG = 180.0 / PI;
constexpr double EARTH_RADIUS = 6371.0; // 地球平均半径(km)
constexpr double IONO_HEIGHT = 350.0;   // 电离层等效高度(km)

// 映射函数类型
enum class MappingFunctionType {
    SLM,    // 单层模型
    MSLM,   // 修正单层模型
    TSM     // 薄壳映射函数
};

STECCalculator::STECCalculator() 
    : ionoHeight_(IONO_HEIGHT), 
      mappingFuncType_(MappingFunctionType::SLM) {
    // 初始化
}

STECCalculator::~STECCalculator() {
    // 清理资源
}

void STECCalculator::setIonosphereHeight(double height) {
    ionoHeight_ = height;
}

void STECCalculator::setMappingFunction(const std::string& mappingFunction) {
    if (mappingFunction == "MSLM") {
        mappingFuncType_ = MappingFunctionType::MSLM;
    } else if (mappingFunction == "TSM") {
        mappingFuncType_ = MappingFunctionType::TSM;
    } else {
        // 默认使用SLM
        mappingFuncType_ = MappingFunctionType::SLM;
    }
}

void STECCalculator::setStationInfos(const std::map<std::string, StationInfo>& stationInfos) {
    stationInfos_ = stationInfos;
}

void STECCalculator::addStationInfo(const std::string& stationName, const StationInfo& info) {
    stationInfos_[stationName] = info;
}

std::vector<STECObservation> STECCalculator::calculateSTEC(const std::vector<GNSSObservation>& observations) {
    std::vector<STECObservation> stecObs;
    
    // 按卫星分组
    auto satelliteGroups = groupObservationsBySatellite(observations);
    
    // 对每个卫星-接收机对，基于周跳标志划分为不同的弧段
    for (auto& [satKey, satObs] : satelliteGroups) {
        // 进一步按接收机分组
        std::map<std::string, std::vector<GNSSObservation>> stationGroups;
        for (const auto& obs : satObs) {
            stationGroups[obs.getStation()].push_back(obs);
        }
        
        // 对每个接收机的观测数据进行处理
        for (auto& [stationName, stationObs] : stationGroups) {
            // 按时间排序
            std::sort(stationObs.begin(), stationObs.end(), 
                     [](const GNSSObservation& a, const GNSSObservation& b) {
                         return a.time < b.time;
                     });
            
            // 划分为不同的弧段
            auto arcs = divideObservationsIntoArcs(stationObs);
            
            // 计算每个弧段的STEC
            for (const auto& arc : arcs) {
                // 确保弧段至少有一定数量的观测
                if (arc.size() < 5) {
                    continue;
                }
                
                // 使用相位对齐到伪距水平计算STEC
                auto arcStecObs = calculatePhaseAlignedSTEC(arc);
                stecObs.insert(stecObs.end(), arcStecObs.begin(), arcStecObs.end());
            }
        }
    }
    
    std::cout << "Calculated " << stecObs.size() << " STEC observations using phase-aligned method" << std::endl;
    return stecObs;
}

// 将观测数据划分为不同的弧段
std::vector<std::vector<GNSSObservation>> STECCalculator::divideObservationsIntoArcs(
    const std::vector<GNSSObservation>& observations) {
    
    std::vector<std::vector<GNSSObservation>> arcs;
    
    if (observations.empty()) {
        return arcs;
    }
    
    // 获取配置参数
    auto& config = Config::getInstance();
    auto& prepSettings = config.preprocessingSettings;
    double maxTimeGap = prepSettings.maxTimeGap;
    
    // 开始一个新的弧段
    std::vector<GNSSObservation> currentArc;
    currentArc.push_back(observations[0]);
    
    // 遍历观测数据，根据周跳标志或时间间隙划分弧段
    for (size_t i = 1; i < observations.size(); ++i) {
        const auto& obs = observations[i];
        const auto& prevObs = observations[i-1];
        
        // 检查是否有周跳或时间间隙过大
        double timeGap = obs.time - prevObs.time;
        
        if (obs.hasCycleSlip || timeGap > maxTimeGap) {
            // 结束当前弧段并开始新的弧段
            if (!currentArc.empty()) {
                arcs.push_back(currentArc);
                currentArc.clear();
            }
        }
        
        // 添加到当前弧段
        currentArc.push_back(obs);
    }
    
    // 添加最后一个弧段
    if (!currentArc.empty()) {
        arcs.push_back(currentArc);
    }
    
    std::cout << "Divided observations into " << arcs.size() << " arcs" << std::endl;
    return arcs;
}

// 使用相位观测值对齐到伪距水平计算STEC
std::vector<STECObservation> STECCalculator::calculatePhaseAlignedSTEC(
    const std::vector<GNSSObservation>& arc) {
    
    std::vector<STECObservation> stecObs;
    
    if (arc.empty()) {
        return stecObs;
    }
    
    // 获取配置参数
    auto& config = Config::getInstance();
    auto& stecSettings = config.stecSettings;
    int windowSize = stecSettings.smoothingWindowSize;
    
    // 计算伪距STEC和相位STEC
    std::vector<double> pseudorangeSTEC;
    std::vector<double> phaseSTEC;
    
    for (const auto& obs : arc) {
        // 计算伪距组合的STEC
        double p_stec = calculatePseudorangeSTEC(obs);
        pseudorangeSTEC.push_back(p_stec);
        
        // 计算相位组合的STEC
        double ph_stec = calculatePhaseSTEC(obs);
        phaseSTEC.push_back(ph_stec);
    }
    
    // 计算相位和伪距STEC的差值（偏移量）
    std::vector<double> offsets;
    for (size_t i = 0; i < arc.size(); ++i) {
        offsets.push_back(pseudorangeSTEC[i] - phaseSTEC[i]);
    }
    
    // 使用滑动窗口平均计算稳定的偏移量
    std::vector<double> smoothedOffsets;
    for (size_t i = 0; i < arc.size(); ++i) {
        // 确定窗口范围
        int startIdx = std::max(0, static_cast<int>(i) - windowSize/2);
        int endIdx = std::min(static_cast<int>(arc.size()) - 1, static_cast<int>(i) + windowSize/2);
        
        // 计算窗口内的平均偏移
        double sumOffset = 0.0;
        int count = 0;
        for (int j = startIdx; j <= endIdx; ++j) {
            sumOffset += offsets[j];
            count++;
        }
        
        double avgOffset = (count > 0) ? sumOffset / count : 0.0;
        smoothedOffsets.push_back(avgOffset);
    }
    
    // 应用平滑偏移量，计算最终的STEC
    for (size_t i = 0; i < arc.size(); ++i) {
        const auto& obs = arc[i];
        
        // 相位STEC加上平均偏移量得到校准的STEC
        double calibratedSTEC = phaseSTEC[i] + smoothedOffsets[i];
        
        // 创建STEC观测结果
        STECObservation stecObservation;
        stecObservation.time = obs.time;
        stecObservation.satellite = obs.getSatellite();
        stecObservation.station = obs.getStation();
        stecObservation.elevation = obs.elevation;
        stecObservation.azimuth = obs.azimuth;
        
        // 计算电离层穿刺点
        auto ippCoords = calculateIonosphericPiercePoint(obs);
        stecObservation.latitude = ippCoords[0];
        stecObservation.longitude = ippCoords[1];
        
        stecObservation.stec = calibratedSTEC;
        stecObservation.stecSigma = 0.1;  // 相位STEC通常比伪距STEC精度高
        
        stecObs.push_back(stecObservation);
    }
    
    return stecObs;
}

std::vector<double> STECCalculator::calculateIonosphericPiercePoint(const GNSSObservation& obs) {
    // 获取站点坐标
    double stationLat = 0.0;
    double stationLon = 0.0;
    double stationHeight = 0.0;
    
    // 如果stationPosition已经设置，直接使用
    if (obs.stationPosition.size() >= 3) {
        // 从ECEF坐标转换为大地坐标
        std::vector<double> ecef = {obs.stationPosition[0], obs.stationPosition[1], obs.stationPosition[2]};
        std::vector<double> llh = Utils::ecef2llh(ecef);
        stationLat = llh[0];
        stationLon = llh[1];
        stationHeight = llh[2];
    } else {
        // 尝试从stationInfo获取
        StationInfo stationInfo;
        if (obs.station >= 0) {
            std::string stationName = obs.getStation();
            // 获取指定名称的测站信息
            stationInfo = getStationInfoByName(stationName);
            
            // 转换ECEF坐标到大地坐标
            std::vector<double> ecef = {stationInfo.x, stationInfo.y, stationInfo.z};
            std::vector<double> llh = Utils::ecef2llh(ecef);
            stationLat = llh[0];
            stationLon = llh[1];
            stationHeight = llh[2];
        }
    }
    
    // 卫星高度角和方位角(弧度)
    double elevation = obs.elevation * DEG2RAD;
    double azimuth = obs.azimuth * DEG2RAD;
    
    // 初始化站点纬度和经度(弧度)
    double stationLatRad = stationLat * DEG2RAD;
    double stationLonRad = stationLon * DEG2RAD;
    
    // 地心角计算
    double psi = (PI/2.0) - elevation - 
                std::asin((EARTH_RADIUS / (EARTH_RADIUS + ionoHeight_)) * std::cos(elevation));
    
    // 计算穿刺点纬度和经度(弧度)
    double ippLat = std::asin(std::sin(stationLatRad) * std::cos(psi) + 
                             std::cos(stationLatRad) * std::sin(psi) * std::cos(azimuth));
    
    double ippLon = stationLonRad + 
                   std::asin(std::sin(psi) * std::sin(azimuth) / std::cos(ippLat));
    
    // 转换为度
    double ippLatDeg = ippLat / DEG2RAD;
    double ippLonDeg = ippLon / DEG2RAD;
    
    // 保持经度在-180到180之间
    if (ippLonDeg > 180.0) ippLonDeg -= 360.0;
    if (ippLonDeg < -180.0) ippLonDeg += 360.0;
    
    return {ippLatDeg, ippLonDeg};
}

std::vector<VTECObservation> STECCalculator::convertSTECToVTEC(const std::vector<STECObservation>& stecObs) {
    std::vector<VTECObservation> vtecObs;
    
    for (const auto& stec : stecObs) {
        // 计算映射函数
        double mappingFunc = calculateMappingFunction(stec.elevation);
        
        // 创建VTEC观测
        VTECObservation vtec;
        vtec.time = stec.time;
        vtec.satellite = stec.satellite;
        vtec.station = stec.station;
        vtec.latitude = stec.latitude;
        vtec.longitude = stec.longitude;
        vtec.elevation = stec.elevation;
        vtec.azimuth = stec.azimuth;
        vtec.stec = stec.stec;
        vtec.mappingFunction = mappingFunc;
        vtec.vtec = stec.stec / mappingFunc;  // STEC / 映射函数 = VTEC
        
        vtecObs.push_back(vtec);
    }
    
    return vtecObs;
}

std::map<int, std::vector<STECObservation>> STECCalculator::groupSTECByTime(
    const std::vector<STECObservation>& stecObs, int timeInterval) {
    
    std::map<int, std::vector<STECObservation>> timeGroups;
    
    for (const auto& obs : stecObs) {
        // 计算观测时间所属的时段索引
        int timeIndex = static_cast<int>(obs.time / timeInterval);
        timeGroups[timeIndex].push_back(obs);
    }
    
    return timeGroups;
}

std::map<int, std::vector<GNSSObservation>> STECCalculator::groupObservationsBySatellite(
    const std::vector<GNSSObservation>& observations) {
    
    std::map<int, std::vector<GNSSObservation>> satelliteGroups;
    
    for (const auto& obs : observations) {
        satelliteGroups[obs.satellite].push_back(obs);
    }
    
    return satelliteGroups;
}

double STECCalculator::calculateMappingFunction(double elevation) {
    // 转换为弧度
    double elev = elevation * DEG2RAD;
    
    switch (mappingFuncType_) {
        case MappingFunctionType::MSLM:
            return calculateMSLMMappingFunction(elev);
        case MappingFunctionType::TSM:
            return calculateTSMMappingFunction(elev);
        case MappingFunctionType::SLM:
        default:
            return calculateSLMMappingFunction(elev);
    }
}

// 单层模型(SLM)映射函数
double STECCalculator::calculateSLMMappingFunction(double elevation) {
    double z = PI / 2.0 - elevation;  // 天顶角
    double Re = EARTH_RADIUS;         // 地球半径
    double Ri = Re + ionoHeight_;     // 电离层高度
    
    double sinZp = (Re / Ri) * std::sin(z);
    double zp = std::asin(sinZp);     // 穿刺点天顶角
    
    return 1.0 / std::cos(zp);
}

// 修正单层模型(MSLM)映射函数
double STECCalculator::calculateMSLMMappingFunction(double elevation) {
    // 修正单层模型考虑了地球曲率
    double z = PI / 2.0 - elevation;  // 天顶角
    double Re = EARTH_RADIUS;         // 地球半径
    double Ri = Re + ionoHeight_;     // 电离层高度
    
    // 计算地心角
    double psi = asin((Re / Ri) * sin(z));
    
    // 计算穿刺点天顶角
    double zp = z - psi;
    
    // 计算穿刺点到电离层路径长度与垂直路径的比率
    double mapFunc = (Ri * cos(zp)) / cos(z) / ionoHeight_;
    
    return mapFunc;
}

// 薄壳映射函数(TSM - Thin Shell Model)
double STECCalculator::calculateTSMMappingFunction(double elevation) {
    // 薄壳模型基于更简化的几何方程
    double z = PI / 2.0 - elevation;  // 天顶角
    
    // 添加纬度相关的修正项 (使用固定参数，可以通过配置进行调整)
    double a0 = 1.0;
    double a1 = 0.15;
    double a2 = 0.0018;
    
    return (a0 + a1 * pow(z, 1.0) + a2 * pow(z, 2.0)) / cos(z);
}

// 考虑方位角的映射函数 (添加水平梯度支持)
double STECCalculator::calculateMappingFunctionWithAzimuth(double elevation, double azimuth, 
                                                          double latGrad, double lonGrad) {
    double baseMapping = calculateMappingFunction(elevation);
    
    // 方位角转弧度
    double azimuthRad = azimuth * DEG2RAD;
    
    // 基于方位角添加水平梯度项
    // latGrad和lonGrad是纬度和经度方向的梯度参数
    double azimuthTerm = latGrad * cos(azimuthRad) + lonGrad * sin(azimuthRad);
    
    // 添加到基本映射函数
    return baseMapping * (1.0 + azimuthTerm);
}

double STECCalculator::calculateSTECValue(double L1, double L2, double f1, double f2) {
    // 从载波相位观测计算STEC
    // STEC = (f1^2 * f2^2) / (40.3 * (f1^2 - f2^2)) * (L1*lambda1 - L2*lambda2)
    double lambda1 = LIGHT_SPEED / f1;
    double lambda2 = LIGHT_SPEED / f2;
    
    double factor = (f1*f1 * f2*f2) / (TECU_FACTOR * (f1*f1 - f2*f2));
    double stec = factor * (L1 * lambda1 - L2 * lambda2);
    
    return stec;
}

double STECCalculator::calculatePseudorangeSTEC(const GNSSObservation& obs) {
    // 计算伪距组合STEC
    // STEC = (f1^2 * f2^2) / (40.3 * (f2^2 - f1^2)) * (P2 - P1 - DCB_r - DCB^s)
    // 注意：这里暂时不考虑DCB影响，后续通过联合估计解算DCB
    double f1 = obs.f1;
    double f2 = obs.f2;
    double P1 = obs.P1;
    double P2 = obs.P2;
    
    double factor = (f1*f1 * f2*f2) / (TECU_FACTOR * (f2*f2 - f1*f1));
    double stec = factor * (P2 - P1);  // 忽略DCB影响
    
    return stec;
}

double STECCalculator::calculatePhaseSTEC(const GNSSObservation& obs) {
    // 计算相位组合STEC
    // STEC = (f1^2 * f2^2) / (40.3 * (f1^2 - f2^2)) * (L1*lambda1 - L2*lambda2 - lambda1*N1 + lambda2*N2)
    // 注意：相位模糊度未知，这里计算的是相对STEC值，还需要对齐到伪距水平
    double f1 = obs.f1;
    double f2 = obs.f2;
    double L1 = obs.L1;
    double L2 = obs.L2;
    double lambda1 = LIGHT_SPEED / f1;
    double lambda2 = LIGHT_SPEED / f2;
    
    double factor = (f1*f1 * f2*f2) / (TECU_FACTOR * (f1*f1 - f2*f2));
    double stec = factor * (L1 * lambda1 - L2 * lambda2);
    
    return stec;
}

// 实现getStationInfoByName方法
StationInfo STECCalculator::getStationInfoByName(const std::string& stationName) {
    auto it = stationInfos_.find(stationName);
    if (it != stationInfos_.end()) {
        return it->second;
    }
    // 如果找不到，返回空的StationInfo
    return StationInfo();
} 